/*



extern crate nalgebra as na;
use na::*;
use rand::prelude::*;

  let a = Matrix2::new(1, 3, 2, 4);
    let b = Matrix2::new(1, 3, 2, 4);
    let c = a * b;  矩阵乘法
    let d = a.dot(&b); 内积一个数字
    let e = a.transpose();
    let aa = Matrix2::new(1., 2., 3., 4.);
    let f = aa.try_inverse().unwrap();

    let mut rng = rand::thread_rng();
    let x = na::DMatrix::<f64>::new_random(2, 3);
    let x = Matrix2x3::<f64>::zeros();
    let y = DMatrix::<f64>::identity(2,2);
    let Q = na::DMatrix::<f64>::diagonal(&y);


#[macro_export]
macro_rules! lkf_builder {
    ($lkf_wrapper: ty) => {
        impl $lkf_wrapper {
            fn predict(&mut self) {
                // projects state ahead
               self.state.x = self.lk.A * self.state.x + self.lk.B*self.state.u;
                // project error covariance ahead
                self.lk.P = self.lk.A * self.lk.P * self.lk.A.transpose() + self.lk.Q;
            }

             fn update(&mut self) {
                // compute Kalman gain
                let mut inner_val = self.lk.H * self.lk.P * self.lk.H.transpose() + self.lk.R;
                inner_val.try_inverse_mut();
                self.lk.K = self.lk.P * self.lk.H.transpose() * inner_val;

                // update estimate with measurement z
                self.state.x = self.state.x + self.lk.K*(self.state.z - self.lk.H * self.state.x);

                // update error covariance
                self.lk.P = (self.lk.I - self.lk.K * self.lk.H) * self.lk.P;
            }
        }
    }


/// https://blog.csdn.net/robert_chen1988/article/details/104877913   u+C*Z
fn gen_mvn2_from_std(rho: f64) -> (f64, f64) {
    // specify mean and covariance of our multi-variate normal

    let _cov = vec![1.0, rho, rho, 1.0];
    let cov = OMatrix::<f64, U2, U2>::from_vec(_cov);

    //之所以半边不是0因为只有两列两行,右上角就一个0就是半边0
    let cho = Cholesky::<f64, U2>::new_unchecked(cov);
    let z1 = random_from_stdnorm();
    let z2 = random_from_stdnorm();
    let _z = vec![z1, z2];
    let z = OMatrix::<f64, U2, U1>::from_vec(_z);
    let res = cho.l() * z;
    (res[(0, 0)], res[(1, 0)])
}


*/
